#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/PointCloud2.h>
#include <Eigen/Dense>
#include <pcl/common/common.h>

class PointCloudToGridNode {
public:
    PointCloudToGridNode() : cloud_received_(false) {
        ros::NodeHandle nh;
        ros::NodeHandle private_nh("~");
        
        // 参数
        private_nh.param<double>("resolution", resolution_, 0.1);
        private_nh.param<double>("min_height", min_height_, -0.5);
        private_nh.param<double>("max_height", max_height_, 0.5);
        private_nh.param<double>("occupied_threshold", occupied_threshold_, 1);
        private_nh.param<int>("grid_width", grid_width_, 1000);
        private_nh.param<int>("grid_height", grid_height_, 1000);
        
        // 订阅点云地图
        cloud_sub_ = nh.subscribe("/static_map_cloud", 1, &PointCloudToGridNode::cloudCallback, this);
        
        // 发布占用栅格地图
        map_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("/map", 1, true);
        
        ROS_INFO("PointCloud to OccupancyGrid node initialized");
    }
    
    void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
        ROS_INFO("Received point cloud with %d points", cloud_msg->width * cloud_msg->height);
        
        // 将ROS消息转换为PCL点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromROSMsg(*cloud_msg, *cloud);
        
        // 找到点云的边界
        Eigen::Vector4f min_pt, max_pt;
        pcl::getMinMax3D<pcl::PointXYZ>(*cloud, min_pt, max_pt);
        
        // 创建占用栅格地图
        nav_msgs::OccupancyGrid grid;
        grid.header = cloud_msg->header;
        grid.info.resolution = resolution_;
        
        // 设置地图原点为点云的最小点
        grid.info.origin.position.x = min_pt[0];
        grid.info.origin.position.y = min_pt[1];
        grid.info.origin.position.z = 0.0;
        grid.info.origin.orientation.w = 1.0;
        
        // 计算地图尺寸
        double width_m = max_pt[0] - min_pt[0];
        double height_m = max_pt[1] - min_pt[1];
        
        grid.info.width = std::max(10, static_cast<int>(width_m / resolution_));
        grid.info.height = std::max(10, static_cast<int>(height_m / resolution_));
        
        // 限制地图大小
        if (grid_width_ > 0 && grid_height_ > 0) {
            grid.info.width = std::min(grid.info.width, static_cast<uint32_t>(grid_width_));
            grid.info.height = std::min(grid.info.height, static_cast<uint32_t>(grid_height_));
        }
        
        ROS_INFO("Creating grid map with size %dx%d cells", grid.info.width, grid.info.height);
        
        // 初始化地图数据为未知(-1)
        grid.data.resize(grid.info.width * grid.info.height, -1);
        
        // 将点云投影到2D栅格
        for (const auto& point : cloud->points) {
            // 只考虑指定高度范围内的点
            if (point.z < min_height_ || point.z > max_height_) {
                continue;
            }
            
            // 计算点在栅格中的索引
            int grid_x = static_cast<int>((point.x - grid.info.origin.position.x) / resolution_);
            int grid_y = static_cast<int>((point.y - grid.info.origin.position.y) / resolution_);
            
            // 检查索引是否在范围内
            if (grid_x >= 0 && grid_x < grid.info.width && grid_y >= 0 && grid_y < grid.info.height) {
                int index = grid_y * grid.info.width + grid_x;
                
                // 标记为占用(100)
                grid.data[index] = 100;
            }
        }
        
        // 发布占用栅格地图
        map_pub_.publish(grid);
        ROS_INFO("Published occupancy grid map");
        
        cloud_received_ = true;
    }
    
private:
    ros::Subscriber cloud_sub_;
    ros::Publisher map_pub_;
    double resolution_;
    double min_height_;
    double max_height_;
    double occupied_threshold_;
    int grid_width_;
    int grid_height_;
    bool cloud_received_;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "pointcloud_to_grid_node");
    PointCloudToGridNode node;
    ros::spin();
    return 0;
} 